#pragma once

typedef struct { float x, y, z, w; } Quat; /* Quaternion */
enum QuatPart { X, Y, Z, W };
typedef Quat EulerAngles;
typedef float HMatrix[4][4];

/*** Order type constants, constructors, extractors ***/
/* There are 24 possible conventions, designated by:    */
/*	  o EulAxI = axis used initially		    */
/*	  o EulPar = parity of axis permutation		    */
/*	  o EulRep = repetition of initial axis as last	    */
/*	  o EulFrm = frame from which axes are taken	    */
/* Axes I,J,K will be a permutation of X,Y,Z.	    */
/* Axis H will be either I or K, depending on EulRep.   */
/* Frame S takes axes from initial static frame.	    */
/* If ord = (AxI=X, Par=Even, Rep=No, Frm=S), then	    */
/* {a,b,c,ord} means Rz(c)Ry(b)Rx(a), where Rz(c)v	    */
/* rotates v around Z by c radians.			    */
#define EulFrmS	     0
#define EulFrmR	     1
#define EulFrm(ord)  ((unsigned)(ord)&1)
#define EulRepNo     0
#define EulRepYes    1
#define EulRep(ord)  (((unsigned)(ord)>>1)&1)
#define EulParEven   0
#define EulParOdd    1
#define EulPar(ord)  (((unsigned)(ord)>>2)&1)
#define EulSafe	     "\000\001\002\000"
#define EulNext	     "\001\002\000\001"
#define EulAxI(ord)  ((int)(EulSafe[(((unsigned)(ord)>>3)&3)]))
#define EulAxJ(ord)  ((int)(EulNext[EulAxI(ord)+(EulPar(ord)==EulParOdd)]))
#define EulAxK(ord)  ((int)(EulNext[EulAxI(ord)+(EulPar(ord)!=EulParOdd)]))
#define EulAxH(ord)  ((EulRep(ord)==EulRepNo)?EulAxK(ord):EulAxI(ord))
/* EulGetOrd unpacks all useful information about order simultaneously. */
#define EulGetOrd(ord,i,j,k,h,n,s,f) {unsigned o=ord;f=o&1;o>>=1;s=o&1;o>>=1;\
    n=o&1;o>>=1;i=EulSafe[o&3];j=EulNext[i+n];k=EulNext[i+1-n];h=s?k:i;}
/* EulOrd creates an order value between 0 and 23 from 4-tuple choices. */
#define EulOrd(i,p,r,f)	   (((((((i)<<1)+(p))<<1)+(r))<<1)+(f))
/* Static axes */
#define EulOrdXYZs    EulOrd(X,EulParEven,EulRepNo,EulFrmS)
#define EulOrdXYXs    EulOrd(X,EulParEven,EulRepYes,EulFrmS)
#define EulOrdXZYs    EulOrd(X,EulParOdd,EulRepNo,EulFrmS)
#define EulOrdXZXs    EulOrd(X,EulParOdd,EulRepYes,EulFrmS)
#define EulOrdYZXs    EulOrd(Y,EulParEven,EulRepNo,EulFrmS)
#define EulOrdYZYs    EulOrd(Y,EulParEven,EulRepYes,EulFrmS)
#define EulOrdYXZs    EulOrd(Y,EulParOdd,EulRepNo,EulFrmS)
#define EulOrdYXYs    EulOrd(Y,EulParOdd,EulRepYes,EulFrmS)
#define EulOrdZXYs    EulOrd(Z,EulParEven,EulRepNo,EulFrmS)
#define EulOrdZXZs    EulOrd(Z,EulParEven,EulRepYes,EulFrmS)
#define EulOrdZYXs    EulOrd(Z,EulParOdd,EulRepNo,EulFrmS)
#define EulOrdZYZs    EulOrd(Z,EulParOdd,EulRepYes,EulFrmS)
/* Rotating axes */
#define EulOrdZYXr    EulOrd(X,EulParEven,EulRepNo,EulFrmR)
#define EulOrdXYXr    EulOrd(X,EulParEven,EulRepYes,EulFrmR)
#define EulOrdYZXr    EulOrd(X,EulParOdd,EulRepNo,EulFrmR)
#define EulOrdXZXr    EulOrd(X,EulParOdd,EulRepYes,EulFrmR)
#define EulOrdXZYr    EulOrd(Y,EulParEven,EulRepNo,EulFrmR)
#define EulOrdYZYr    EulOrd(Y,EulParEven,EulRepYes,EulFrmR)
#define EulOrdZXYr    EulOrd(Y,EulParOdd,EulRepNo,EulFrmR)
#define EulOrdYXYr    EulOrd(Y,EulParOdd,EulRepYes,EulFrmR)
#define EulOrdYXZr    EulOrd(Z,EulParEven,EulRepNo,EulFrmR)
#define EulOrdZXZr    EulOrd(Z,EulParEven,EulRepYes,EulFrmR)
#define EulOrdXYZr    EulOrd(Z,EulParOdd,EulRepNo,EulFrmR)
#define EulOrdZYZr    EulOrd(Z,EulParOdd,EulRepYes,EulFrmR)


EulerAngles Eul_(float ai, float aj, float ah, int order);
Quat Eul_ToQuat(EulerAngles ea);
void Eul_ToHMatrix(EulerAngles ea, HMatrix M);
EulerAngles Eul_FromHMatrix(HMatrix M, int order);
EulerAngles Eul_FromQuat(Quat q, int order);

class CEulDataHanlder
{
public:
	CEulDataHanlder();
	~CEulDataHanlder();

	template<typename T>
	static void QaternionToRotationMatrix(T *vQuaternion, T*vRotationMatrixm);

	template<typename T>
	static void Vec3MatrixMult(T* vVec3, T* vMatrix);

	static float RadiansToDegrees(float fRadians);
};

template <typename T>
void CEulDataHanlder::Vec3MatrixMult(T* vVec3, T* vMatrix)
{
	T vec3_x = vVec3[0] * vMatrix[0] + vVec3[1] * vMatrix[3] + vVec3[2] * vMatrix[6];
	T vec3_y = vVec3[0] * vMatrix[1] + vVec3[1] * vMatrix[4] + vVec3[2] * vMatrix[7];
	T vec3_z = vVec3[0] * vMatrix[2] + vVec3[1] * vMatrix[5] + vVec3[2] * vMatrix[8];
	vVec3[0] = vec3_x;
	vVec3[1] = vec3_y;
	vVec3[2] = vec3_z;
}

template<typename T>
void CEulDataHanlder::QaternionToRotationMatrix(T *vQaternion, T *vRotationMatrix)
{
	vRotationMatrix[0] = 1 - 2 * vQaternion[Y] * vQaternion[Y] - 2 * vQaternion[Z] * vQaternion[Z]; vRotationMatrix[3] = 2 * vQaternion[X] * vQaternion[Y] - 2 * vQaternion[W] * vQaternion[Z];   vRotationMatrix[6] = 2 * vQaternion[X] * vQaternion[Z] + 2 * vQaternion[W] * vQaternion[Y];
	vRotationMatrix[1] = 2 * vQaternion[X] * vQaternion[Y] + 2 * vQaternion[W] * vQaternion[Z];   vRotationMatrix[4] = 1 - 2 * vQaternion[X] * vQaternion[X] - 2 * vQaternion[Z] * vQaternion[Z]; vRotationMatrix[7] = 2 * vQaternion[Y] * vQaternion[Z] - 2 * vQaternion[W] * vQaternion[X];
	vRotationMatrix[2] = 2 * vQaternion[X] * vQaternion[Z] - 2 * vQaternion[W] * vQaternion[Y];   vRotationMatrix[5] = 2 * vQaternion[Y] * vQaternion[Z] + 2 * vQaternion[W] * vQaternion[X];   vRotationMatrix[8] = 1 - 2 * vQaternion[X] * vQaternion[X] - 2 * vQaternion[Y] * vQaternion[Y];
}

